#include <iostream>
#include "rigidBody.h"
#include "ODE.h"
#include <cmath>

bool normRows=false;
bool normCols=false;
bool areColliding(MyVec3 cubePosition, float cubeSize,MyVec3 spherePosition, float sphereRadius)
{
    MyVec3 delta =distTwoPoints(cubePosition, spherePosition);
    float terms=pow(cubeSize/1.1,2);//maybe divide cubesize by 2 if collisions are too soon. math says 2, but visuals say no
    terms*=3;
    float cubeBound=sqrt(terms);
     //cout<<cubeBound<<endl;
    if(delta.getMagnitude()<=sphereRadius+cubeBound || delta.getMagnitude()<=cubeBound+sphereRadius)
    
    {
        return true;
    }
    return false;
}


void normCol(float* ort)
{
    int i;
    float col1=0;
    float col2=0;
    float col3=0;
    
    for(i=0;i<12;i++)
    {
        if(i<3)
        {
            col1+=(ort[i]*ort[i]);
        }
        else if(i>3&&i<8)
        {
            col2+=(ort[i]*ort[i]);
        }
        else if(i>7)
        {
            col3+=(ort[i]*ort[i]);
        }
    }
    col1=sqrt(col1);
    col2=sqrt(col2);
    col3=sqrt(col3);
    for(i=0;i<3; i++)
    {
        ort[i]/=(col1);
        ort[i+4]/=(col2);
        ort[i+8]/=(col3);
        
    }
   
}
void normRow(float* ort)
{
 float row1= pow(ort[0],2)+pow(ort[4],2)+pow(ort[8],2)+pow(ort[12],2);
  float row2= pow(ort[1],2)+pow(ort[5],2)+pow(ort[9],2)+pow(ort[13],2);
   float row3= pow(ort[2],2)+pow(ort[6],2)+pow(ort[10],2)+pow(ort[14],2); 
    float row4= pow(ort[3],2)+pow(ort[7],2)+pow(ort[11],2)+pow(ort[15],2);
 row1=sqrt(row1);
  row2=sqrt(row2);
   row3=sqrt(row3);
    row4=sqrt(row4);
  
        ort[0]/=row1;
        ort[4]/=row1;
        ort[8]/=row1;
        ort[12]/=row1;
        
        ort[1]/=row2;
        ort[5]/=row2;
        ort[9]/=row2;
        ort[13]/=row2;
        
        ort[2]/=row3;
        ort[6]/=row3;
        ort[10]/=row3;
        ort[14]/=row3;
        
        ort[3]/=row4;
        ort[7]/=row4;
        ort[11]/=row4;
        ort[15]/=row4;
}





void RigidBody::renderSelf(){
     render();
}

//void RigidBody::ODEify(float h){}

vector<MyVec3> RigidBody::getState(){
    vector<MyVec3> result = vector<MyVec3>();
    result.push_back(this->position);
    result.push_back(this->linMomentum);  
    return result;
}

vector<MyVec3> RigidBody::getDerivState(){
    vector<MyVec3> result = vector<MyVec3>();
    result.push_back(this->linMomentum / this->mass);
    result.push_back(this->forceTotal);
    return result;
}

void RigidBody::setState(vector<MyVec3> state){
    this->position = state[0];
    this->linMomentum = state[1];
}

void RigidBody::clearForces(){
    forceTotal.n[0] = 0;
    forceTotal.n[1] = 0;
    forceTotal.n[2] = 0;
}


SphereBody::SphereBody(float r, float x, float y, float z, float m){
    size2=r;
    radius = r;
    mass = m;
    inertiaTensor = MyMat3(((2/5)/(this->mass*this->radius*this->radius)),0,0,0,((2/5)/(this->mass*this->radius*this->radius)),0,0,0,((2/5)/(this->mass*this->radius*this->radius)));
    linMomentum = MyVec3();
    angMomentum = MyVec3(1,0,0);
    forceTotal = MyVec3();
    torque = MyVec3();
    ort = MyMat3(1,0,0,0,1,0,0,0,1);
    position.n[0] = x;
    position.n[1] = y;
    position.n[2] = z;                     
}

CubeBody::CubeBody(float s, float x, float y, float z, float m){
    size2=s;
    size = s;
    mass = m;
    inertiaTensor = MyMat3(2.0/12,0,0,
                           0,2.0/12,0,
                           0,0,2.0/12);

    linMomentum = MyVec3();
    angMomentum = MyVec3(1,0,0);
    forceTotal = MyVec3();
    torque = MyVec3();
    ort = MyMat3(1,0,0,
                 0,1,0,
                 0,0,1);
    position.n[0] = x;
    position.n[1] = y;
    position.n[2] = z;                     

}

void SphereBody::render(){
     GLfloat mat_ambient[] = {0.2, 0.2, 0.2, 1.0};
     GLfloat mat_diffuse[] = {0.8, 0.8, 0.8, 1.0};
     GLfloat mat_specular[] = { 1.0, 1.0, 1.0, 1.0 };
     GLfloat mat_shininess[] = { 50.0 };
     glClearColor (0.0, 0.0, 0.0, 0.0);

     glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient);
     glMaterialfv(GL_FRONT, GL_AMBIENT, mat_diffuse);
     glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);
     glMaterialfv(GL_FRONT, GL_SHININESS, mat_shininess);
     glPushMatrix();
     glTranslatef(position.n[0], position.n[1], position.n[2]);
     float rot[] = {ort.n[0],ort.n[3],ort.n[6],0,ort.n[1],ort.n[4],ort.n[7],0,ort.n[2],ort.n[5],ort.n[8],0,0,0,0,1};
//booboobooboobo
     if(normCols)normCol(rot);
     if(normRows)normRow(rot);
     glMultMatrixf(rot);
     glutSolidSphere(radius,30,30);
     glPopMatrix();
}

void CubeBody::render(){
     GLfloat mat_ambient[] = {0.2, 0.2, 0.2, 1.0};
     GLfloat mat_diffuse[] = {0.8, 0.8, 0.8, 1.0};
     GLfloat mat_specular[] = { 1.0, 1.0, 1.0, 1.0 };
     GLfloat mat_shininess[] = { 50.0 };
     glClearColor (0.0, 0.0, 0.0, 0.0);

     glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient);
     glMaterialfv(GL_FRONT, GL_AMBIENT, mat_diffuse);
     glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);
     glMaterialfv(GL_FRONT, GL_SHININESS, mat_shininess);
     glPushMatrix();
     glTranslatef(position.n[0], position.n[1], position.n[2]);
     float rot[] = {ort.n[0],ort.n[3],ort.n[6],0,ort.n[1],ort.n[4],ort.n[7],0,ort.n[2],ort.n[5],ort.n[8],0,0,0,0,1};
     if(normCols)normCol(rot);
     if(normRows)normRow(rot);
     //BooBooBooBooBooBooBooBooBoo
     glMultMatrixf(rot);
     glutSolidCube(size);
     glPopMatrix();
}

            
        
    

void SphereBody::ODEify(float h)
{
     //update the first parameter in state
     position = position + (linMomentum/mass)*h;

     //update the second parameter in state
     //ort = ort + ODESolve(angMomentum, inertiaTensor, ort, h);
     //ort.printOut();
     
     //update the third parameter in state
     linMomentum = linMomentum+(forceTotal*h);
     
     //update the fourth parameter in state
     angMomentum = angMomentum+(torque*h);
}

void CubeBody::ODEify(float h)
{
     //update the first parameter in state
     position = position + (linMomentum/mass)*h;

     //update the second parameter in state
     ort = ort + ODESolve(angMomentum, inertiaTensor, ort, h);
     
     //update the third parameter in state
     linMomentum = linMomentum+(forceTotal*h);
     
     //update the fourth parameter in state
     angMomentum = angMomentum+(torque*h);
}

